
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/stage_creep.h"
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/traffic_light_left_turn_waiting_zone.h"
#include <string>

#include "modules/common_msgs/perception_msgs/traffic_light_detection.pb.h"
#include "cyber/common/log.h"
#include "modules/planning/planning_base/common/frame.h"
#include "modules/planning/planning_base/common/planning_context.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::hdmap::PathOverlap;
using apollo::perception::TrafficLight;

bool TrafficLightLeftTurnWaitingZoneStageCreep::Init(
        const StagePipeline& config,
        const std::shared_ptr<DependencyInjector>& injector,
        const std::string& config_dir,
        void* context) {
    CHECK_NOTNULL(context);
    bool ret = Stage::Init(config, injector, config_dir, context);
    if (!ret) {
        AERROR << Name() << "init failed!";
        return false;
    }
    return ret;
}

StageResult TrafficLightLeftTurnWaitingZoneStageCreep::Process(
        const TrajectoryPoint& planning_init_point,
        Frame* frame) {
    CHECK_NOTNULL(frame);
    CHECK_NOTNULL(context_);

    auto* context = GetContextAs<TrafficLightLeftTurnWaitingZoneContext>();
    const auto& scenario_config = context->scenario_config;
    auto& reference_line_info = frame->mutable_reference_line_info()->front();
    reference_line_info.LimitCruiseSpeed(scenario_config.creep_speed());
    StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
    if (result.HasError()) {
        AERROR << "TrafficLightLeftTurnWaitingZoneStageCreep planning error";
    }
    hdmap::PathOverlap forward_traffic_light_overlap;
    hdmap::PathOverlap left_turn_traffic_light_overlap_first;
    hdmap::PathOverlap left_turn_traffic_light_overlap_second;
    const std::vector<hdmap::PathOverlap> traffic_light_overlaps
            = reference_line_info.reference_line().map_path().signal_overlaps();
    for (const auto& overlap : traffic_light_overlaps) {
        if (overlap.object_id == context->forward_traffic_light_id) {
            forward_traffic_light_overlap = overlap;
        } else if (overlap.object_id == context->left_turn_traffic_light_id) {
            if (left_turn_traffic_light_overlap_second.start_s < overlap.start_s) {
                std ::swap(left_turn_traffic_light_overlap_first, left_turn_traffic_light_overlap_second);
                left_turn_traffic_light_overlap_second = overlap;
            } else {
                left_turn_traffic_light_overlap_first = overlap;
            }
        }
    }
    const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
    bool is_passed_left_turn_stop_line = adc_front_edge_s > left_turn_traffic_light_overlap_second.end_s;
    auto left_signal_color = frame->GetSignal(context->left_turn_traffic_light_id).color();
    auto done_traffic_light = injector_->planning_context()->mutable_planning_status()->mutable_traffic_light();
    done_traffic_light->mutable_done_traffic_light_overlap_id()->Clear();
    if (is_passed_left_turn_stop_line || left_signal_color == TrafficLight::GREEN) {
        reference_line_info.LimitCruiseSpeed(scenario_config.cruise_speed());
        return FinishStage();
    }

    return result.SetStageStatus(StageStatusType::RUNNING);
}

StageResult TrafficLightLeftTurnWaitingZoneStageCreep::FinishStage() {
    next_stage_ = "TRAFFIC_LIGHT_LEFT_TURN_WAITING_ZONE_CRUISE";
    return StageResult(StageStatusType::FINISHED);
}

}  // namespace planning
}  // namespace apollo
